{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "import sys\n",
    "sys.path.insert(0,'../')\n",
    "sys.path.insert(0,'../../../data_analysis/')\n",
    "from simulate_door_slam import *\n",
    "from trajectory import *\n",
    "from utils import *\n",
    "from local_plot import *\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "path = \"/home/xuhao/data/d2slam/quadcam_7inch_n3_2023_1_14/outputs/doorslam-5-yaw\"\n",
    "nodes = [1, 2, 3, 4, 5]\n",
    "thres = 0.01 #Terminate thresold for DGS\n",
    "evaluate_door_slam(path, nodes, thres, step=10)\n",
    "\n",
    "# paths_pgo, t0 = read_paths(path + \"/door-slam/\", nodes, prefix=\"pgo_\")\n",
    "# paths_rt, _ = read_paths(path + \"/door-slam/\", nodes, prefix=\"realtime_\", t0=t0)\n",
    "# paths_gt, _ = read_paths(path + \"/../../\", nodes, prefix=\"eight_yaw_\", suffix=\"-groundtruth.txt\", t0=t0)\n",
    "# align_paths(paths_rt, paths_gt, align_by_first=True, align_with_minize=True)\n",
    "# align_paths(paths_pgo, paths_gt, align_by_first=True, align_with_minize=True)\n",
    "# dte=550\n",
    "# display(plot_fused_err(nodes, paths_rt, paths_gt, poses_pgo=paths_pgo, dte=dte, show=False))\n",
    "# display(relative_pose_err(nodes, paths_rt, paths_gt, dte=dte))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "path = \"/home/xuhao/data/d2slam/tum_datasets//outputs/door-slam\"\n",
    "nodes = [1, 2, 3, 4, 5]\n",
    "thres = 0.01 #Terminate thresold for DGS\n",
    "evaluate_door_slam(path, nodes, thres, step=10)\n",
    "\n",
    "# paths_pgo, t0 = read_paths(path + \"/door-slam/\", nodes, prefix=\"pgo_\")\n",
    "# paths_rt, _ = read_paths(path + \"/door-slam/\", nodes, prefix=\"realtime_\", t0=t0)\n",
    "# paths_gt, _ = read_paths(path + \"/../../\", nodes, prefix=\"groundtruth_\", t0=t0)\n",
    "# align_paths(paths_rt, paths_gt, align_by_first=True, align_with_minize=True)\n",
    "# align_paths(paths_pgo, paths_gt, align_by_first=True, align_with_minize=True)\n",
    "# dte=550\n",
    "# display(plot_fused_err(nodes, paths_rt, paths_gt, poses_pgo=paths_pgo, dte=dte, show=False))\n",
    "# display(relative_pose_err(nodes, paths_rt, paths_gt, dte=dte))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "path = \"/home/xuhao/data/d2slam/tum_corr/outputs/door-slam\"\n",
    "nodes = [1, 2, 3, 4, 5]\n",
    "thres = 0.01 #Terminate thresold for DGS\n",
    "rt_paths = evaluate_door_slam(path, nodes, thres)\n",
    "paths_pgo, t0 = read_paths(path + \"/door-slam/\", nodes, prefix=\"pgo_\")\n",
    "paths_rt, _ = read_paths(path + \"/door-slam/\", nodes, prefix=\"realtime_\", t0=t0)\n",
    "paths_gt, _ = read_paths(path + \"/../../\", nodes, prefix=\"groundtruth_\", t0=t0)\n",
    "align_paths(paths_rt, paths_gt, align_by_first=True, align_with_minize=True)\n",
    "align_paths(paths_pgo, paths_gt, align_by_first=True, align_with_minize=True)\n",
    "dte=550\n",
    "display(plot_fused_err(nodes, paths_rt, paths_gt, poses_pgo=paths_pgo, dte=dte, show=True))\n",
    "display(relative_pose_err(nodes, paths_rt, paths_gt, dte=dte))\n",
    "plot_fused(nodes, paths_rt, paths_gt, poses_pgo=paths_pgo, figsize=(15,10))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "path = \"/home/xuhao/data/d2slam/tum_corr/outputs/d2slam\"\n",
    "nodes = [1, 2, 3, 4, 5]\n",
    "rt_paths = evaluate_door_slam(path, nodes, 0.01, step=20)\n",
    "paths_pgo, t0 = read_paths(path + \"/door-slam/\", nodes, prefix=\"pgo_\")\n",
    "paths_rt, _ = read_paths(path + \"/door-slam/\", nodes, prefix=\"realtime_\", t0=t0)\n",
    "paths_gt, _ = read_paths(path + \"/../../\", nodes, prefix=\"groundtruth_\", t0=t0)\n",
    "align_paths(paths_rt, paths_gt, align_by_first=True, align_with_minize=True)\n",
    "align_paths(paths_pgo, paths_gt, align_by_first=True, align_with_minize=True)\n",
    "dte=550\n",
    "# display(plot_fused_err(nodes, paths_rt, paths_gt, poses_pgo=paths_pgo, dte=dte, show=True))\n",
    "display(relative_pose_err(nodes, paths_rt, paths_gt, dte=dte))\n",
    "plot_fused(nodes, paths_rt, paths_gt, poses_pgo=paths_pgo, figsize=(15,10))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pgo = PoseGraph()\n",
    "pgo.read_g2o_folder(\"/home/xuhao/data/d2slam/tum_corr/outputs/d2slam/door-slam/rot_initalized/\")\n",
    "frame_id_to_stamp = {}\n",
    "for i in nodes:\n",
    "    with open(f\"/home/xuhao/data/d2slam/tum_corr/outputs/d2slam/swarm{i}/frame_timestamp.txt\", \"r\") as f:\n",
    "        for line in f:\n",
    "            frame_id, stamp = line.split()\n",
    "        frame_id_to_stamp[int(frame_id)] = float(stamp)\n",
    "pgo_latest_states = {i:[] for i in nodes}\n",
    "for agent_id in pgo.agents:\n",
    "    # Find the keyframe with largest frame_id\n",
    "    agent = pgo.agents[agent_id]\n",
    "    robot_idx = agent_id + 1\n",
    "    keyframe_ids = agent.get_keyframe_ids()\n",
    "    for frame_id in keyframe_ids:\n",
    "        ts = frame_id_to_stamp[frame_id]\n",
    "        frame = pgo.keyframes[frame_id]\n",
    "        pos, quat = frame.pos, frame.quat\n",
    "        # Inverse transform by the first keyframe\n",
    "        pgo_latest_states[robot_idx].append(np.concatenate(([ts], pos, quat)))\n",
    "    pgo_latest_states[robot_idx] = np.array(pgo_latest_states[robot_idx])\n",
    "np.savetxt(f\"/home/xuhao/data/d2slam/tum_corr/outputs/d2slam/door-slam/rot_initalized/pgo_{i}.csv\", pgo_latest_states[i], delimiter=\" \")\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "base",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5"
  },
  "orig_nbformat": 4,
  "vscode": {
   "interpreter": {
    "hash": "08ce52785f0fedc81003ce387e097a83d6cc9494681cd746006386992005bb71"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
